#include "Plane.h"
#include "Vector3.h"
#include "Vector4.h"
#include "Matrix3.h"
#include "Math.h"

namespace Core
{
	rtPlane rtPlane::plane_origin(0.0f, 0.0f, 0.0f, 0.0f);

	inline rtPlane::rtPlane(void) 
	{
	}

	inline rtPlane::rtPlane(float a, float b, float c, float d) 
	{
		this->m_A = a;
		this->m_B = b;
		this->m_C = c;
		this->m_D = d;
	}

	inline rtPlane::rtPlane(const rtVector3& normal, const float dist) 
	{
		this->m_A = normal.x;
		this->m_B = normal.y;
		this->m_C = normal.z;
		this->m_D = -dist;
	}

	inline float rtPlane::operator [] (int index) const 
	{
		return (&m_A)[ index ];
	}

	inline float& rtPlane::operator [] (int index) 
	{
		return (&m_A)[ index ];
	}

	inline rtPlane rtPlane::operator - () const 
	{
		return rtPlane(-m_A, -m_B, -m_C, -m_D);
	}

	inline rtPlane& rtPlane::operator = (const rtVector3& v) 
	{ 
		m_A = v.x;
		m_B = v.y;
		m_C = v.z;
		m_D = 0;
		return* this;
	}

	inline rtPlane rtPlane::operator + (const rtPlane& p) const 
	{
		return rtPlane(m_A + p.m_A, m_B + p.m_B, m_C + p.m_C, m_D + p.m_D);
	}

	inline rtPlane rtPlane::operator - (const rtPlane& p) const 
	{
		return rtPlane(m_A - p.m_A, m_B - p.m_B, m_C - p.m_C, m_D - p.m_D);
	}

	inline rtPlane& rtPlane::operator*=(const rtMatrix3& m) 
	{
		normal() *= m;
		return* this;
	}

	inline bool rtPlane::compare(const rtPlane& p) const 
	{
		return (m_A == p.m_A && m_B == p.m_B && m_C == p.m_C && m_D == p.m_D);
	}

	inline bool rtPlane::compare(const rtPlane& p, const float epsilon) const 
	{
		if (rtMath::Fabs(m_A - p.m_A) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(m_B - p.m_B) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(m_C - p.m_C) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(m_D - p.m_D) > epsilon) 
		{
			return false;
		}

		return true;
	}

	inline bool rtPlane::compare(const rtPlane& p, const float normalEps, const float distEps) const 
	{
		if (rtMath::Fabs(m_D - p.m_D) > distEps) 
		{
			return false;
		}
		if (!normal().compare(p.normal(), normalEps)) 
		{
			return false;
		}
		return true;
	}

	inline bool rtPlane::operator == (const rtPlane& p) const 
	{
		return compare(p);
	}

	inline bool rtPlane::operator != (const rtPlane& p) const 
	{
		return !compare(p);
	}

	inline void rtPlane::zero(void) 
	{
		m_A = m_B = m_C = m_D = 0.0f;
	}

	inline void rtPlane::setNormal(const rtVector3& normal) 
	{
		m_A = normal.x;
		m_B = normal.y;
		m_C = normal.z;
	}

	inline const rtVector3& rtPlane::normal(void) const 
	{
		return* reinterpret_cast<const rtVector3*>(&m_A);
	}

	inline rtVector3& rtPlane::normal(void) 
	{
		return* reinterpret_cast<rtVector3*>(&m_A);
	}

	inline float rtPlane::normalize(bool fixDegenerate) 
	{
		float length = reinterpret_cast<rtVector3*>(&m_A)->normalize();

		if (fixDegenerate) 
		{
			fixDegenerateNormal();
		}
		return length;
	}

	inline bool rtPlane::fixDegenerateNormal(void) 
	{
		return normal().fixDegenerateNormal();
	}

	inline bool rtPlane::fixDegeneracies(float distEpsilon) 
	{
		bool fixedNormal = fixDegenerateNormal();
		// only fix dist if the normal was degenerate
		if (fixedNormal) 
		{
			if (rtMath::Fabs(m_D - rtMath::Rint(m_D)) < distEpsilon) 
			{
				m_D = rtMath::Rint(m_D);
			}
		}
		return fixedNormal;
	}

	inline float rtPlane::dist(void) const 
	{
		return -m_D;
	}

	inline void rtPlane::setDist(const float dist) 
	{
		m_D = -dist;
	}

	inline bool rtPlane::fromPoints(const rtVector3& p1, const rtVector3& p2, const rtVector3& p3, bool fixDegenerate) 
	{
		normal() = (p1 - p2).cross(p3 - p2);
		if (normalize(fixDegenerate) == 0.0f) 
		{
			return false;
		}
		m_D = -(normal() * p2);
		return true;
	}

	inline bool rtPlane::fromVecs(const rtVector3& dir1, const rtVector3& dir2, const rtVector3& p, bool fixDegenerate) 
	{
		normal() = dir1.cross(dir2);
		if (normalize(fixDegenerate) == 0.0f) 
		{
			return false;
		}
		m_D = -(normal() * p);
		return true;
	}

	inline void rtPlane::fitThroughPoint(const rtVector3& p) 
	{
		m_D = -(normal() * p);
	}

	inline rtPlane rtPlane::translate(const rtVector3& translation) const 
	{
		return rtPlane(m_A, m_B, m_C, m_D - translation * normal());
	}

	inline rtPlane& rtPlane::translateSelf(const rtVector3& translation) 
	{
		m_D -= translation * normal();
		return *this;
	}

	inline rtPlane rtPlane::rotate(const rtVector3& origin, const rtMatrix3& axis) const 
	{
		rtPlane p;
		p.normal() = normal() * axis;
		p.m_D = m_D + origin * normal() - origin * p.normal();
		return p;
	}

	inline rtPlane& rtPlane::rotateSelf(const rtVector3& origin, const rtMatrix3& axis) 
	{
		m_D += origin * normal();
		normal() *= axis;
		m_D -= origin * normal();
		return *this;
	}

	inline float rtPlane::distance(const rtVector3& v) const 
	{
		return m_A * v.x + m_B * v.y + m_C * v.z + m_D;
	}

	inline PlaneSide rtPlane::side(const rtVector3& v, const float epsilon) const 
	{
		float dist = distance(v);
		if (dist > epsilon) 
		{
			return PLANESIDE_FRONT;
		}
		else if (dist < -epsilon) 
		{
			return PLANESIDE_BACK;
		}
		else 
		{
			return PLANESIDE_ON;
		}
	}

	inline bool rtPlane::lineIntersection(const rtVector3& start, const rtVector3& end) const 
	{
		float d1, d2, fraction;

		d1 = normal() * start + m_D;
		d2 = normal() * end + m_D;
		if (d1 == d2) 
		{
			return false;
		}
		if (d1 > 0.0f && d2 > 0.0f) 
		{
			return false;
		}
		if (d1 < 0.0f && d2 < 0.0f) 
		{
			return false;
		}
		fraction = (d1 / (d1 - d2));
		return (fraction >= 0.0f && fraction <= 1.0f);
	}

	inline bool rtPlane::rayIntersection(const rtVector3& start, const rtVector3& dir, float& scale) const 
	{
		float d1, d2;

		d1 = normal() * start + m_D;
		d2 = normal() * dir;
		if (d2 == 0.0f) 
		{
			return false;
		}
		scale = -(d1 / d2);
		return true;
	}

	inline int rtPlane::getDimension(void) const 
	{
		return 4;
	}

	inline const rtVector4& rtPlane::toVec4(void) const 
	{
		return* reinterpret_cast<const rtVector4*>(&m_A);
	}

	inline rtVector4& rtPlane::toVec4(void) 
	{
		return* reinterpret_cast<rtVector4*>(&m_A);
	}

	inline const float* rtPlane::toFloatPtr(void) const 
	{
		return reinterpret_cast<const float*>(&m_A);
	}

	inline float* rtPlane::toFloatPtr(void) 
	{
		return reinterpret_cast<float*>(&m_A);
	}

	
	PlaneType rtPlane::type(void) const 
	{
		if (normal()[0] == 0.0f) 
		{
			if (normal()[1] == 0.0f) 
			{
				return normal()[2] > 0.0f ? PLANETYPE_Z : PLANETYPE_NEGZ;
			}
			else if (normal()[2] == 0.0f) 
			{
				return normal()[1] > 0.0f ? PLANETYPE_Y : PLANETYPE_NEGY;
			}
			else 
			{
				return PLANETYPE_ZEROX;
			}
		}
		else if (normal()[1] == 0.0f) 
		{
			if (normal()[2] == 0.0f) 
			{
				return normal()[0] > 0.0f ? PLANETYPE_X : PLANETYPE_NEGX;
			}
			else 
			{
				return PLANETYPE_ZEROY;
			}
		}
		else if (normal()[2] == 0.0f) 
		{
			return PLANETYPE_ZEROZ;
		}
		else 
		{
			return PLANETYPE_NONAXIAL;
		}
	}

	

	bool rtPlane::heightFit(const rtVector3* points, const int numPoints) 
	{
		int i;
		float sumXX = 0.0f, sumXY = 0.0f, sumXZ = 0.0f;
		float sumYY = 0.0f, sumYZ = 0.0f;
		rtVector3 sum, average, dir;

		if (numPoints == 1) 
		{
			m_A = 0.0f;
			m_B = 0.0f;
			m_C = 1.0f;
			m_D = -points[0].z;
			return true;
		}
		if (numPoints == 2) 
		{
			dir = points[1] - points[0];
			normal() = dir.cross(rtVector3(0, 0, 1)).cross(dir);
			normalize();
			m_D = -(normal()* points[0]);
			return true;
		}

		sum.zero();
		for (i = 0; i < numPoints; i++) 
		{
			sum += points[i];
		}
		average = sum / numPoints;

		for (i = 0; i < numPoints; i++) 
		{
			dir = points[i] - average;
			sumXX += dir.x* dir.x;
			sumXY += dir.x* dir.y;
			sumXZ += dir.x* dir.z;
			sumYY += dir.y* dir.y;
			sumYZ += dir.y* dir.z;
		}

		if (sumXX * sumYY - sumXY * sumXY < MATRIX_INVERSE_EPSILON) 
		{
			return false;
		}

		m_A = - sumXZ * sumXX - sumYZ * sumXY;
		m_B = - sumXZ * sumXY - sumYZ * sumYY;
		m_C = 1.0f;
		normalize();
		m_D = -(m_A * average.x + m_B * average.y + m_C * average.z);
		return true;
	}

	

	bool rtPlane::planeIntersection(const rtPlane& plane, rtVector3& start, rtVector3& dir) const 
	{
		double n00, n01, n11, det, invDet, f0, f1;

		n00 = normal().lengthSqr();
		n01 = normal()* plane.normal();
		n11 = plane.normal().lengthSqr();
		det = n00* n11 - n01* n01;

		if (rtMath::Fabs(det) < 1e-6f) 
		{
			return false;
		}

		invDet = 1.0f / det;
		f0 = (n01 * plane.m_D - n11 * m_D) * invDet;
		f1 = (n01 * m_D - n00 * plane.m_D) * invDet;

		dir = normal().cross(plane.normal());
		start = f0 * normal() + f1* plane.normal();
		return true;
	}

}

